#!/usr/bin/env python
import roslib; roslib.load_manifest('test_manipulation')

import sys

import rospy
from cs665_manipulation_srvs.srv import *
from geometry_msgs.msg import PointStamped

# try to ask point_arm_client to point
def call_point_arm_client(frame_id, x, y, z):
    rospy.wait_for_service('point_arm_service')
    try:
        point_arm_service = rospy.ServiceProxy('point_arm_service', ArmPointService)
        #create PointStamped
        point = PointStamped()
        point.header.frame_id=frame_id
        point.point.x = x
        point.point.y = y
        point.point.z = z      
        return point_arm_service(point)
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

def drop():
    rospy.wait_for_service('drop_service')
    try:
        drop_service = rospy.ServiceProxy('drop_service', ArmDropService)   
        return drop_service()
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e


def usage():
    return "give a frame and three ints [frame, x, y, z]"

if __name__ == "__main__":
    if len(sys.argv) == 5:
        frame_id = sys.argv[1]
        x = float(sys.argv[2])
        y = float(sys.argv[3])
        z = float(sys.argv[4])
        call_point_arm_client(frame_id, x, y, z)
    elif len(sys.argv) == 2:
        drop()
    else:
        print usage()
        sys.exit(1)
